#include "mpu6050.h"

//IO22:MPU_SCL    IO19:MPU_SDA

//MPU6050原始数据
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

//滤波数据
float Zero_Bias = 0;
float Gyro_x,Gyro_y,Gyro_z;//陀螺仪暂存
float Angle_gx,Angle_gy,Angle_gz;//由角速度计算的倾斜角度
float Accel_x,Accel_y,Accel_z;//加速度暂存
float Angle_ax,Angle_ay,Angle_az;//由加速度计算的倾斜角度
float Angle;//角度暂存
float Angle_x,Angle_y,Angle_z;//小车最终倾斜角度

#define OUTPUT_READABLE_ACCELGYRO

void MPU6050_Init(void)
{
    Wire.begin(MPU_SDA,MPU_SCL);
    Wire.setClock(400000);
    
    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void MPU6050_Print(void)
{
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    Angle_Calculate();

    // display tab-separated accel/gyro x/y/z values
    Serial.print("a/g:\t");
    Serial.print(ax);Serial.print("\t");
    Serial.print(ay);Serial.print("\t");
    Serial.print(az);Serial.print("\t");
    Serial.print(gx);Serial.print("\t");
    Serial.print(gy);Serial.print("\t");
    Serial.print(gz);Serial.print("\n");

    Serial.print("Angle:\t");
    Serial.print(Angle_x);Serial.print("\t");
    Serial.print(Angle_y);Serial.print("\t");
    Serial.print(Angle_z);Serial.print("\n\n");

    delay(200);
}

//倾角计算
void Angle_Calculate(void)
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //范围为2g时，换算关系：16384 LSB/g
    //角度较小时，x = sinx得到角度（弧度）, deg = rad*180/3.14
    //因为x >= sinx,故乘以1.3适当放大
    Accel_x  = ax;
    Angle_ax = (Accel_x - 1100) / 16384;//去除零点偏移,计算得到角度（弧度）
    Angle_ax = Angle_ax * 1.4 * 180 / 3.14;//弧度转换为度

    Accel_y  = ay;
    Angle_ay = (Accel_y - 1100) / 16384;
    Angle_ay = Angle_ay * 1.4 * 180 / 3.14;//弧度转换为度

    Accel_z  = az;
    Angle_az = (Accel_z - 1100) / 16384;
    Angle_az = Angle_az * 1.4 * 180 / 3.14;

    //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
    Gyro_x = gx;
    Gyro_x = -(Gyro_x + Zero_Bias) / 16.4;//去除零点偏移,计算角速度值,负号为方向处理
    //Angle_gy = Angle_gy + Gyro_y * 0.01;//角速度积分得到倾斜角度

    Gyro_y = gy;
    Gyro_y = -(Gyro_y + Zero_Bias) / 16.4;

    Gyro_z = gz;
    Gyro_z = -(Gyro_z + Zero_Bias) / 16.4;
    
    Angle_x = Kalman_Filter(Angle_ax,Gyro_x);//进行卡尔曼滤波
    Angle_y = Kalman_Filter(Angle_ay,Gyro_y);
    Angle_z = Kalman_Filter(Angle_az,Gyro_z);

    
}

//卡尔曼滤波
float Kalman_Filter(float Accel,float Gyro)
{
    static const float Q_angle = 0.001;
	static const float Q_gyro = 0.003;
	static const float R_angle = 0.5;
	static const float dt = 0.01;
	static const char  C_0 = 1;
	static float Q_bias, Angle_err;
	static float PCt_0, PCt_1, E;
	static float K_0, K_1, t_0, t_1;
	static float Pdot[4] = {0,0,0,0};
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };

    Angle += (Gyro - Q_bias) * dt; //先验估计

    Pdot[0]=Q_angle - PP[0][1] - PP[1][0];// Pk-先验估计误差协方差的微分
    
    Pdot[1] =- PP[1][1];
    Pdot[2] =- PP[1][1];
    Pdot[3] = Q_gyro;
    
    PP[0][0] += Pdot[0] * dt;// Pk-先验估计误差协方差微分的积分
    PP[0][1] += Pdot[1] * dt;// =先验估计误差协方差
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;
    
    Angle_err = Accel - Angle;
    //zk-先验估计  
    
    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];
    
    E = R_angle + C_0 * PCt_0;
    
    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;
    
    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];
    
    PP[0][0] -= K_0 * t_0;
    //后验估计误差协方差
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;
    
    Angle += K_0 * Angle_err;//后验估计
    Q_bias += K_1 * Angle_err;//后验估计
    Gyro -= Q_bias;//输出值(后验估计)的微分 = 角速度

    return Gyro;
}

